//
// Created by Ukua on 2024/3/25.
//

#include "dm_motors.h"

static uint16_t idx = 0;
INTF_Motor_HandleTypeDef *g_DM_motors[DM_MOTOR_NUM] = {0};

osThreadId DM_MainLoopTaskHandle;

//#define OFFSET_ANGLE 0.396692792f

#define MASTER_ID 0x00


//反馈帧
struct DM_feedback_frame {
    uint8_t id_with_error:8;//电机id和错误信息 ID|ERR<<4
    uint16_t POS:16;//位置
    uint16_t VEL:12;//速度
    uint16_t T:12;//力矩
    uint8_t T_MOS:8;//MOS温度 单位摄氏度
    uint8_t T_Rotor:8;//电机内部线圈平均温度 单位摄氏度
};

//MIT控制帧
struct DM_MIT_frame {
    uint16_t P_des:16;//位置给定
    uint16_t V_des:12;//速度给定
    uint16_t Kp:12;//kp 0~500
    uint16_t Kd:12;//kd 0~5
    uint16_t T_ff:12;//力矩给定
};


void DM_Enable(INTF_Motor_HandleTypeDef *motor, Bus_TopicHandleTypeDef *can_tx_topic) {
    INTF_CAN_MessageTypeDef msg = {
            msg.id_type = BSP_CAN_ID_STD,
            msg.can_id = motor->motor_id,
    };
    msg.data[0] = 0xFF;
    msg.data[1] = 0xFF;
    msg.data[2] = 0xFF;
    msg.data[3] = 0xFF;
    msg.data[4] = 0xFF;
    msg.data[5] = 0xFF;
    msg.data[6] = 0xFF;
    msg.data[7] = 0xFC;//使能帧
    Bus_Publish(can_tx_topic, &msg);
}

void DM_ControlMode(INTF_Motor_HandleTypeDef *motor, float torque, float MechPosition, float speed, float kp, float kd,
                           Bus_TopicHandleTypeDef *can_tx_topic) {

    DM_ResDataTypeDef *priv = (DM_ResDataTypeDef *) motor->private_data;

    struct DM_MIT_frame frame = {
        .P_des = float_to_uint(MechPosition, priv->const_res.P_MIN, priv->const_res.P_MAX, 16),
        .V_des = float_to_uint(speed, priv->const_res.V_MIN, priv->const_res.V_MAX, 12),
        .Kp = float_to_uint(kp, priv->const_res.KP_MIN, priv->const_res.KP_MAX, 12),
        .Kd = float_to_uint(kd, priv->const_res.KD_MIN, priv->const_res.KD_MAX, 12),
        .T_ff = float_to_uint(torque, priv->const_res.T_MIN, priv->const_res.T_MAX, 12),
    };

    INTF_CAN_MessageTypeDef msg = {
            .id_type=BSP_CAN_ID_STD,
            .can_id= motor->motor_id,
    };
//    memcpy(msg.data, &frame, 8);h
    msg.data[0] = (frame.P_des >> 8);
    msg.data[1] = frame.P_des;
    msg.data[2] = (frame.V_des >> 4);
    msg.data[3] = ((frame.V_des&0xF)<<4)|(frame.Kp>>8);
    msg.data[4] = frame.Kp;
    msg.data[5] = (frame.Kd >> 4);
    msg.data[6] = ((frame.Kd&0xF)<<4)|(frame.T_ff>>8);
    msg.data[7] = frame.T_ff;
    Bus_Publish(can_tx_topic, &msg);
}

void DM_Setmode_t(struct INTF_Motor_Handle *self, INTF_Motor_ModeTypeDef mode) {
    return;
}

void DM_SetSpeed_t(struct INTF_Motor_Handle *self, float speed) {
    self->target_speed = speed;
}

void DM_SetAngle_t(struct INTF_Motor_Handle *self, float angle) {
    self->target_angle = angle;
}

void DM_SetTorque_t(struct INTF_Motor_Handle *self, float torque) {
    self->target_torque = torque;
}

void DM_Reset_t(struct INTF_Motor_Handle *self) {
}

void DM_Enable_t(struct INTF_Motor_Handle *self) {
}

void DM_Disable_t(struct INTF_Motor_Handle *self) {
}


void DM_MainLoop() {
    osDelay(3000); //等待电机启动
    for (int i = 0; i < DM_MOTOR_NUM; i++) {
        DM_Enable(g_DM_motors[i],
                         ((DM_ResDataTypeDef *) g_DM_motors[i]->private_data)->can_tx_topic);
        osDelay(250);
    }
    osDelay(1000);
    while (1) {
        for (int i = 0; i < DM_MOTOR_NUM; i++) {
            INTF_Motor_HandleTypeDef *m = g_DM_motors[i];
            DM_ResDataTypeDef *priv = (DM_ResDataTypeDef *) g_DM_motors[i]->private_data;

            DM_ControlMode(m, m->target_torque * m->direction,
                                  (m->target_angle - priv->angle_offset) * m->direction,//修正offset后的角度值
                                  m->target_speed * m->direction,
                                  priv->kp,
                                  priv->kd,
                                  priv->can_tx_topic);

//            DM_ControlMode(m->motor_id, 0.0f,
//                                 0.0f,//修正offset后的角度值
//                                 0.0f,
//                                 0.0f,
//                                 0.0f,
//                                 priv->can_tx_topic);
            osDelay(0);
        }
//        char a[50];
//        extern float INS_angle[3];
//        sprintf(a,"%f\n",INS_angle[1]);
//        LOG(a);

        osDelay(2);
    }
}

void DM_CAN_CallBack(void *message, Bus_TopicHandleTypeDef *topic) {
    INTF_CAN_MessageTypeDef *msg = (INTF_CAN_MessageTypeDef *) message;

    if (msg->id_type == BSP_CAN_ID_EXT || msg->can_id != MASTER_ID) {
        return;
    }

    INTF_Motor_HandleTypeDef *m = NULL;
    int i;
    for (i = 0; i < DM_MOTOR_NUM; i++) {
        //由于达妙的神奇id设计，只能通过id的低四位来匹配电机
        if ((g_DM_motors[i]->motor_id & 0xF) == (msg->data[0] & 0xF) &&
        //无err的情况下可以判断8位 id|err<<4
//        if ((g_DM_motors[i]->motor_id & 0xFF) == (msg->data[0] & 0xFF) &&
            ((DM_ResDataTypeDef *) g_DM_motors[i]->private_data)->can_rx_topic->topic == topic) {

            m = g_DM_motors[i];
            break;
        }

        //未匹配，直接返回
        if (i == DM_MOTOR_NUM - 1) {
            return;
        }
    }
    DM_ResDataTypeDef *priv = (DM_ResDataTypeDef *) g_DM_motors[i]->private_data;

    struct DM_feedback_frame frame;
    frame.POS=(msg->data[1]<<8)|msg->data[2];
    frame.VEL=(msg->data[3]<<4)|(msg->data[4]>>4);
    frame.T=((msg->data[4]&0xF)<<8)|msg->data[5];
    m->real_angle = m->direction*uint_to_float(frame.POS, priv->const_res.P_MIN, priv->const_res.P_MAX, 16) + m->angle_offset;
    m->real_speed = m->direction*uint_to_float(frame.VEL, priv->const_res.V_MIN, priv->const_res.V_MAX, 12);
    m->real_torque = m->direction*uint_to_float(frame.T, priv->const_res.T_MIN, priv->const_res.T_MAX, 12);
}


void DM_Register(DM_ConfigTypeDef *config) {
    INTF_Motor_HandleTypeDef *motor = Bus_SharePtr(config->motor_ptr_name, sizeof(INTF_Motor_HandleTypeDef));
    motor->motor_id = config->motor_id;
    motor->motor_mode = config->motor_mode;
    motor->motor_state = MOTOR_STATE_INIT;
    motor->target_speed = 0.0f;
    motor->real_speed = 0.0f;
    motor->target_angle = 0.0f;
    motor->real_angle = 0.0f;
    motor->target_torque = 0.0f;
    motor->real_torque = 0.0f;
    motor->angle_offset = config->angle_offset;
    motor->direction = config->direction;

    motor->set_torque = DM_SetTorque_t;
    motor->set_speed = DM_SetSpeed_t;
    motor->set_angle = DM_SetAngle_t;
    motor->set_mode = DM_Setmode_t;

//    motor->private_data = JUST_MALLOC(sizeof(DM_ResDataTypeDef));
    DM_ResDataTypeDef *priv = (struct DM_ResData *) JUST_MALLOC(sizeof(struct DM_ResData));
    memset(priv, 0, sizeof(DM_ResDataTypeDef));
    motor->private_data = priv;

    priv->motor_type = config->motor_type;
    switch (config->motor_type) {
        case DM_J8006:
            memcpy(priv->const_res.data, DM_J8006_ConstRes, sizeof(union DM_ConstRes));
            break;
        case DM_H6215:
            memcpy(priv->const_res.data, DM_H6215_ConstRes, sizeof(union DM_ConstRes));
            break;
        default:
            ///未知的电机类型
            while(1);
    }

    priv->can_rx_topic = Bus_SubscribeFromName(config->can_rx_topic_name,DM_CAN_CallBack);
    priv->can_tx_topic = Bus_TopicRegister(config->can_tx_topic_name);
    priv->kp = config->kp;
    priv->kd = config->kd;
    g_DM_motors[idx++] = motor;
}


void DM_Init() {
    //注册电机开始
    #define OFFSET_ANGLE 12.98f*DEG2RAD

    DM_ConfigTypeDef leglf_config = {
            .motor_id = 0x11,
            .motor_type = DM_J8006,
            .motor_ptr_name = "/motor/leg-l-f",
            .angle_offset = -1.433013f-OFFSET_ANGLE,//0.658617973f+OFFSET_ANGLE -0.731f,
            .direction = 1.0f,
            .motor_mode = MOTOR_MODE_TORQUE,
            .can_rx_topic_name = "/CAN1/RX",
            .can_tx_topic_name = "/CAN1/TX",
    };
    DM_Register(&leglf_config);

    DM_ConfigTypeDef leglb_config = {
            .motor_id = 0x13,
            .motor_type = DM_J8006,
            .motor_ptr_name = "/motor/leg-l-b",
            .angle_offset = 0.851645f+PI+OFFSET_ANGLE,//0.847829819f+PI-OFFSET_ANGLE,
            .direction = 1.0f,
            .motor_mode = MOTOR_MODE_TORQUE,
            .can_rx_topic_name = "/CAN1/RX",
            .can_tx_topic_name = "/CAN1/TX",
    };
    DM_Register(&leglb_config);

    DM_ConfigTypeDef legrb_config = {
            .motor_id = 0x14,
            .motor_type = DM_J8006,
            .motor_ptr_name = "/motor/leg-r-b",
            .angle_offset = -2.351225f+PI+OFFSET_ANGLE,//2.08342838f+PI-OFFSET_ANGLE-2.696071f+PI,
            .direction = -1.0f,
            .motor_mode = MOTOR_MODE_TORQUE,
            .can_rx_topic_name = "/CAN1/RX",
            .can_tx_topic_name = "/CAN1/TX",
    };
    DM_Register(&legrb_config);

    DM_ConfigTypeDef legrf_config = {
            .motor_id = 0x12,
            .motor_type = DM_J8006,
            .motor_ptr_name = "/motor/leg-r-f",
            .angle_offset = 1.622606f-OFFSET_ANGLE,//-3.83211327f+OFFSET_ANGLE+2.06f,
            .direction = -1.0f,
            .motor_mode = MOTOR_MODE_TORQUE,
            .can_rx_topic_name = "/CAN1/RX",
            .can_tx_topic_name = "/CAN1/TX",
    };
    DM_Register(&legrf_config);

    DM_ConfigTypeDef legmr_config = {
            .motor_id = 0x28,
            .motor_type = DM_H6215,
            .motor_ptr_name = "/motor/leg-MR",
            .angle_offset = 0,
            .direction = -1.0f,
            .motor_mode = MOTOR_MODE_TORQUE,
            .can_rx_topic_name = "/CAN1/RX",
            .can_tx_topic_name = "/CAN1/TX",
    };
    DM_Register(&legmr_config);

    DM_ConfigTypeDef legml_config = {
            .motor_id = 0x29,
            .motor_type = DM_H6215,
            .motor_ptr_name = "/motor/leg-ML",
            .angle_offset = 0,
            .direction = 1.0f,
            .motor_mode = MOTOR_MODE_TORQUE,
            .can_rx_topic_name = "/CAN1/RX",
            .can_tx_topic_name = "/CAN1/TX",
    };
    DM_Register(&legml_config);
    //注册电机结束

    osThreadDef(DM_MainLoopTask, DM_MainLoop, osPriorityLow, 0, 256);
    DM_MainLoopTaskHandle = osThreadCreate(osThread(DM_MainLoopTask), NULL);
}


